package it.gius.pe.shapes;

import it.gius.pe.aabb.AABoundaryBox;
import it.gius.pe.shapes.witness.EllipsePointWitness;
import it.gius.pe.shapes.witness.Witness;

import org.jbox2d.common.Mat22;
import org.jbox2d.common.Transform;
import org.jbox2d.common.Vec2;

public class Ellipse extends Shape{

	private float a,b;
	private Vec2 ab,a_b;
	
	private float aa,bb;

	private Vec2 centerPosition;

	private Vec2 pool1 = new Vec2();
	private Vec2 pool2 = new Vec2();

	public Vec2 velocity;

	private float angle;
	public float angularSpeed;

	private Transform transform;

	public Ellipse(float a, float b, Vec2 pos) throws IllegalArgumentException{

		this(a,b,pos,0);
	}

	public Ellipse(float a, float b, Vec2 pos, float angle) throws IllegalArgumentException{

		if(b>a)
			throw new IllegalArgumentException();

		this.a = a;
		this.b = b;
		
		aa = a*a;
		bb = b*b;

		ab = new Vec2(a,b);
		a_b = new Vec2(a,-b);

		centerPosition = new Vec2();
		centerPosition.set(pos);

		velocity = new Vec2();
		velocity.setZero();


		angularSpeed = 0;

		this.angle = angle;
		
		
		transform = new Transform();
		
		transform.set(centerPosition, angle);
	}
	
	public Vec2 getCenterPosition() {
		return centerPosition;
	}

	public void setCenterPosition(Vec2 centerPosition) {
		this.centerPosition.set(centerPosition);
		transform.position.set(centerPosition);
	}
	
	public void setCenterPosition(float x, float y) {
		this.centerPosition.set(x,y);
		transform.position.set(x,y);
	}

	public float getAngle() {
		return angle;
	}

	public void setAngle(float angle) {
		this.angle = angle;
		transform.R.set(angle);
	}
	
	public Transform getTransform()
	{
		return transform;
	}


	public float getA() {
		return a;
	}

	public void setAB(float a,float b) {
		if(b>a)
			throw new IllegalArgumentException();

		this.a = a;
		this.b = b;
		
		aa = a*a;
		bb = b*b;

		ab.set(a,b);
		a_b.set(a,-b);

	}

	public float getB() {
		return b;
	}


	
	@Override
	public void getBoundaryBox(AABoundaryBox result) throws BadShapeException {
		
		Mat22 R =  transform.R;
		
		pool1.x = (float)StrictMath.sqrt(R.col1.x*R.col1.x*aa + R.col1.y*R.col1.y*bb);
		pool1.y = (float)StrictMath.sqrt(R.col2.x*R.col2.x*aa + R.col2.y*R.col2.y*bb);
		
		pool2.set(centerPosition);
		pool2.subLocal(pool1);
		
		result.lowerBound.set(pool2);
		
		pool2.set(centerPosition);
		pool2.addLocal(pool1);
		
		result.upperBound.set(pool2);
		
	}

	/*@Override
	public void getBoundaryBox(AABoundaryBox result) throws BadShapeException {
		
		//transform.R.set(angle);
		//ab.x = a; ab.y = b;
		//a_b.x = a; a_b.y = -b;

		transform.R.mulToOut(ab, ab);
		transform.R.mulToOut(a_b, a_b);

		result.upperBound.x = Math.max(Math.abs(ab.x), Math.abs(a_b.x));
		result.upperBound.y = Math.max(Math.abs(ab.y), Math.abs(a_b.y));

		result.lowerBound.x = -result.upperBound.x;
		result.lowerBound.y = -result.upperBound.y;
		
		result.lowerBound.addLocal(centerPosition);
		result.upperBound.addLocal(centerPosition);

	}*/
	
	public void ccTangent(Witness witness,Vec2 tangentResult)
	{
		EllipsePointWitness ewit = (EllipsePointWitness)witness;
		
		tangentResult.x = -a*ewit.eY/b;
		tangentResult.y = b*ewit.eX/a;
		
		//transform.R.set(angle);
		
		transform.R.mulToOut(tangentResult, tangentResult);
	}
	

	public void normal(Witness witness,Vec2 normalResult)
	{
		EllipsePointWitness ewit = (EllipsePointWitness)witness;
		
		//xNormal = yTangent (counter-clockwise)
		//yNormal = -xTangent  (counter-clockwise)
		
		
		normalResult.x = b*ewit.eX/a; 
		normalResult.y = a*ewit.eY/b; 
		
		//transform.R.set(angle);
		
		transform.R.mulToOut(normalResult, normalResult);
	}

	@Override
	public void fromWitnessToVec(Witness witness,Vec2 result) {

		if(witness == null)
			throw new IllegalArgumentException();
		else
		{
			EllipsePointWitness ewit = (EllipsePointWitness)witness;
			//float x = a*(float)StrictMath.cos(ewit.angle);
			//float y = b*(float)StrictMath.sin(ewit.angle);
			result.set(ewit.eX,ewit.eY);
		}

		//transform.set(this.centerPosition, this.angle);

		Transform.mulToOut(transform, result, result);
	}

}
